//
// Created by jg on 2021/3/2.
//
#include "path_test.h"

namespace al_topo_map{
    double TOPO_MAP::getFirstNumber(std::string &line) {
        double result;

        std::size_t index = line.find_first_of(' ');
        if(index != std::string::npos){
            result = atof(line.substr(0, index).c_str());
            line   = line.substr(index, line.size()-index);
        } else{
            result = atof(line.c_str());
            return result;
        }

        index = line.find_first_not_of(' ');
        if(index != std::string::npos)
            line  = line.substr(index, line.size()-index);

        return result;
    }

    void TOPO_MAP::readFile(std::string file_name) {
        std::string err_file = file_name;
        int index = err_file.find_last_of('/');
        err_file = err_file.substr(0, index);
        err_file += "/GNSS_POS.err";

        thread read_GNSS_thread(&TOPO_MAP::readGNSSData, this, file_name);
        thread read_ERR_thread(&TOPO_MAP::readPOSERR,    this,  err_file);

        read_GNSS_thread.join();
        read_ERR_thread.join();

        //保留与参考真值做差的点
        auto node_iter = node_vec_.begin();
        for(; node_iter != node_vec_.end(); ){
            if(err_vec_.empty()) {
                while(node_iter != node_vec_.end())
                    node_vec_.erase(node_iter);

                break;
            }

            auto& node = *node_iter;
            auto& node_t = node->time_;

            auto err_iter = err_vec_.begin();
            auto& err = *err_iter;
            if(node_t >= err->time_ - 0.5 && node_t <= err->time_ + 0.5){
                node->setERR(err->e_N_, err->e_E_, err->e_D_);
                err_vec_.erase(err_iter);
                ++node_iter;
            }else if(node_t > err->time_){
                err_vec_.erase(err_iter);
            }else {
                node_vec_.erase(node_iter);
            }
        }

    }

    void TOPO_MAP::readGNSSData(std::string file_name) {

        std::ifstream fp;
        fp.open(file_name);
        if(!fp){
            std::cerr << "open the GNSS data file is error ! " << std::endl;
            exit(-1);
        }

        //清空
        vector<MAP_NODE::Ptr>().swap(node_vec_);

        std::string line;
        double time, B, L, H, e_N, e_E, e_D;
        Eigen::Vector3d BLH, pos;
        while(getline(fp, line)){
            //time
            time = getFirstNumber(line);
            //B
            B = getFirstNumber(line);
            //L
            L = getFirstNumber(line);
            //H
            H = getFirstNumber(line);
            //e_N
            e_N = getFirstNumber(line);
            //e_E
            e_E = getFirstNumber(line);
            //e_D
            e_D = getFirstNumber(line);

            BLH << B*D2R, L*D2R, H;
            MAP_NODE::Ptr node = std::make_shared<MAP_NODE>(BLH, e_N, e_E, e_D);

            blh2Locpos_.BLH2xy(BLH, pos);
            node->loc_pos_ = pos;
            node->time_    = time;

            node_vec_.push_back(node);
        }

        fp.close();
    }

    void TOPO_MAP::readPOSERR(std::string file_name) {
        std::ifstream fp;
        fp.open(file_name);
        if(!fp){
            std::cerr << "open the POS ERR file is error ! " << std::endl;
            exit(-1);
        }

        //清空
        vector<POS_ERR::Ptr>().swap(err_vec_);

        std::string line;
        double time, e_N, e_E, e_D;
        while(getline(fp, line)){
            //time
            time = getFirstNumber(line);
            //e N
            e_N = getFirstNumber(line);
            //e E
            e_E = getFirstNumber(line);
            //e D
            e_D = getFirstNumber(line);

            POS_ERR::Ptr perr = std::make_shared<POS_ERR>(time, e_N, e_E, e_D);
            err_vec_.push_back(perr);
        }

        fp.close();
    }

    void TOPO_MAP::classifyEdge(vector<MAP_NODE::Ptr>& list) {

        vector<EDGE_BOUND>().swap(edge_bounds_);
        // edge index
        // 0 -edge 6 7
        edge_bounds_.emplace_back(EDGE_BOUND(-40.680, -32.874, 4.808, 60.144));
        // 1 -edge 3 6
        edge_bounds_.emplace_back(EDGE_BOUND(-33.988, 1.931, 57.887, 63.261));
        // 2 -edge 3 4
        edge_bounds_.emplace_back(EDGE_BOUND(0.093, 7.017, 6.122, 59.158));
        // 3 -edge 4 7
        edge_bounds_.emplace_back(EDGE_BOUND(-39.733, 6.765, -2.477, 7.808));
        // 4 -edge 7 9
        edge_bounds_.emplace_back(EDGE_BOUND(-59.568, -39.0, -2.488, 7.344));
        // 5 -edge 7 8
        edge_bounds_.emplace_back(EDGE_BOUND(-44.562, -34.473, -60.459, -1.0));
        // 6 -edge 5 8
        edge_bounds_.emplace_back(EDGE_BOUND(-39.897, 5.992, -63.325, -49.112));
        // 7 -edge 4 5
        edge_bounds_.emplace_back(EDGE_BOUND(-1.03, 8.381, -49.566, 2.141));

        auto edge_index = [&](const double x, const double y){
            for(int i = 0; i < (int)edge_bounds_.size(); ++i){
                if(x >= edge_bounds_[i].xmin
                && x <= edge_bounds_[i].xmax
                && y >= edge_bounds_[i].ymin
                && y <= edge_bounds_[i].ymax){
                    return i;
                }
            }
            //范围外
            return -1;
        };

        for(auto& node : list){
            auto& _x = node->loc_pos_[0];
            auto& _y = node->loc_pos_[1];

            int index = edge_index(_x, _y);
            if(index < 0) continue;
            edge_node_[index].push_back(node);
        }

    }

    void TOPO_MAP::run(){
        edge_node_.resize(13);
        readFile(fileA);

        classifyEdge(node_vec_);

        readFile(fileB);

        classifyEdge(node_vec_);

        {
            vector<double> vec_sigma;
            vector<Eigen::Vector3d> vec_RMS;
            double sigma, RMS_N, RMS_E, RMS_D;
            int _size = edge_bounds_.size();
            for (int i = 0; i < _size; ++i) {
                sigma = getSIGMA(edge_node_[i], RMS_N, RMS_E, RMS_D);
                vec_RMS.emplace_back(Eigen::Vector3d(RMS_N, RMS_E, RMS_D));
                vec_sigma.push_back(sigma);
            }

            //以下道路缺少定位数据，单独赋值
            //8 edge 8 10
            vec_sigma.push_back(0.402);
            //9 edge 0 3
            vec_sigma.push_back(3.503);
            //10 edge 0 1
            vec_sigma.push_back(2.059);
            //11 edge 1 2
            vec_sigma.push_back(1.752);
            //12 edge 2 5
            vec_sigma.push_back(3.503);

            sort(vec_err_.begin(), vec_err_.end());

            int size_err = vec_err_.size();
            //sigma 0 根据需求赋值
            double sigma_0 = vec_err_.at(size_err/2) * 2;

            std::cout << "alpha 0 : " << sigma_0 << std::endl;
            for (int i = 0; i < (int)vec_sigma.size(); ++i) {
                auto& a = vec_sigma[i];
                std::cout << i << " " << a << " "
                << a/sigma_0 << " "
                << log10(a/sigma_0) << std::endl;
            }

        }

        //rviz显示
        if(TOPO_MAP::useROS()){
            int num = edge_bounds_.size();

            vector<vector<Eigen::Vector3d>> pos_vec(num);
            for(int i = 0; i < num; i++){
                for(auto& node : edge_node_[i])
                    pos_vec[i].push_back(node->loc_pos_);
            }

            while (ros::ok()){
                for (int i = 0; i < num; ++i) {
                    float color = (float)i / (float)num;
                    makeMap(pos_vec[i], 0.5, Eigen::Vector4d(color, 0.0, 1.0, 1), 0);
                }

                ros::Duration(1.0).sleep();
            }
        }
    }

    double TOPO_MAP::getSIGMA(vector<MAP_NODE::Ptr> &list, double& RMS_N, double& RMS_E, double& RMS_D) {
        auto getNORM = [](double eN, double eE, double eD){
            double result = pow(eN, 2) + pow(eE, 2) + pow(eD, 2);
            result = sqrt(result);

            return result;
        };

        int n = list.size();
        double sumN = 0, sumE = 0, sumD = 0;

        for(auto& node : list){
            auto& eN = node->e_N_;
            auto& eE = node->e_E_;
            auto& eD = node->e_D_;

            sumN += pow(eN, 2);
            sumE += pow(eE, 2);
            sumD += pow(eD, 2);

            vec_err_.push_back(getNORM(eN, eE, eD));

        }

        RMS_N = sqrt(sumN / n);
        RMS_E = sqrt(sumE / n);
        RMS_D = sqrt(sumD / n);

        return getNORM(RMS_N, RMS_E, RMS_D);
    }

    void TOPO_MAP::rvizInit(ros::NodeHandle &nh) {
        ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("/vis_GNSS_pos",  1000);
        pubs_.push_back(marker_pub);
    }

    void TOPO_MAP::makeMap(const vector<Eigen::Vector3d>& list,
                           double resolution,
                           const Eigen::Vector4d& color,
                           int id) {
        visualization_msgs::Marker mk;
        mk.header.frame_id = "odom";
        mk.header.stamp    = ros::Time::now();
        mk.type            = visualization_msgs::Marker::SPHERE_LIST;
//        mk.action          = visualization_msgs::Marker::DELETE;
        mk.id              = 0;
//        vis_marker_pub_.publish(mk);

        mk.action             = visualization_msgs::Marker::ADD;
        mk.pose.orientation.x = 0.0;
        mk.pose.orientation.y = 0.0;
        mk.pose.orientation.z = 0.0;
        mk.pose.orientation.w = 1.0;

        mk.color.r = color(0);
        mk.color.g = color(1);
        mk.color.b = color(2);
        mk.color.a = color(3);

        mk.scale.x = resolution;
        mk.scale.y = resolution;
        mk.scale.z = resolution;

        geometry_msgs::Point pt;
        for (int i = 0; i < int(list.size()); i++) {
            pt.x = list[i](0);
            pt.y = list[i](1);
            pt.z = list[i](2);
            mk.points.push_back(pt);
        }
        pubs_[id].publish(mk);
//        ros::Duration(0.001).sleep();
    }
}

int main(int argc, char** argv){

    al_topo_map::TOPO_MAP::isUSEROS(false);
    al_topo_map::TOPO_MAP topo_map;

    if(al_topo_map::TOPO_MAP::useROS()){
        ros::init(argc, argv, "plan_node");
        ros::NodeHandle nh("~");

        topo_map.rvizInit(nh);
    }

    topo_map.run();


    AL_Path al_path;
    al_path.init(true);
    Eigen::Vector3d start, end;
    start << -50, 0, 0.01;
    end   << 300,  0, 0.01;
    auto path = al_path.findPath(start, end);
    for(auto& p : path)
        std::cout << p.transpose() << std::endl;

    return 0;
}